import math
from ctypes import *

import numpy as np
import numpy.ctypeslib as npct
from numpy.core.defchararray import index
from numpy.core.records import record
import matplotlib.pyplot as plt

#label  index  black 1  green  2 white  3
file = open("/media/st/data_fast/tj/gazebo/03143/testing/trajectory/black.txt", "r")
tra_list = file.readlines()
black_gt = []
for i in range(len(tra_list)):
    black_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
black_gt = np.array(black_gt)
file.close()
file = open("/media/st/data_fast/tj/gazebo/03143/testing/trajectory/green.txt", "r")
tra_list = file.readlines()
brown_gt = []
for i in range(len(tra_list)):
    brown_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
brown_gt = np.array(brown_gt)
file.close()
file = open("/media/st/data_fast/tj/gazebo/03143/testing/trajectory/white.txt", "r")
tra_list = file.readlines()
white_gt = []
for i in range(len(tra_list)):
    white_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
white_gt = np.array(white_gt)
file.close()

#read data record
file = open("/media/st/data_fast/tj/gazebo/03143/sim.txt", "r")
tra_list = file.readlines()
record_data = []
for i in range(len(tra_list)):
    record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
record_data = np.array(record_data)
file.close()

file = open("/media/st/data_fast/tj/gazebo/03143/testing/trajectory/master.txt", "r")
tra_list = file.readlines()
master_gt = []
for i in range(len(tra_list)):
    master_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float32 ))
master_gt = np.array(master_gt)
file.close()

#按照类别将数据进行划分
brown_mea = []
white_mea = []
black_mea = []
for i in range(record_data.shape[0]):
    if record_data[i, 1] == 0:
        black_mea.append(record_data[i, :])
    if record_data[i, 1] == 1:
        brown_mea.append(record_data[i, :])
    if record_data[i, 1] == 2:
        white_mea.append(record_data[i, :])
black_mea = np.array(black_mea)
black_mea = black_mea[10:-10, :]
brown_mea = np.array(brown_mea)
brown_mea = brown_mea[0:-10, :]
white_mea = np.array(white_mea)
white_mea = white_mea[10:-10, :]

#与gps数据对齐
brown_gt[:,0] = brown_gt[:,0]
brown_gt[:,1] = brown_gt[:,1]-0.0

from filterpy.kalman import KalmanFilter

#kf
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.H = np.array([[1,0,0,0,0,0],
                                [0,1,0,0,0,0]])
# kf.P[2:, 2:] *= 0.5. 	# state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
kf.P *= 0.5
kf.Q[0:, 0:] *= 10
kf.R = kf.R*900
kf.x = np.array([brown_mea[0,2],brown_mea[0,3],-2,0.0,0,0])
for i in range(brown_mea.shape[0]):
    delta_t = brown_mea[i,0] - brown_mea[i-1,0]
    kf.F = np.array([[1,0,delta_t,0,0.5*delta_t**2,0],      # state transition matrix
                     [0,1,0,delta_t,0,0.5*delta_t**2],
                     [0,0,1,0,delta_t,0],
                     [0,0,0,1,0,delta_t],
                     [0,0,0,0,1,0],
                     [0,0,0,0,0,1]])
    kf.predict()
    kf.update(brown_mea[i,2:4])
    brown_mea[i,5:7] = kf.x[2:4]

#gt
kf_gt = KalmanFilter(dim_x=6, dim_z=2)
kf_gt.H = np.array([[1,0,0,0,0,0],
                    [0,1,0,0,0,0]])
kf_gt.P[2:, 2:] *= 0.1	# state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
kf_gt.P *= 0.1
kf_gt.Q[0:, 0:] *= 0.01
kf_gt.R = kf_gt.R*0.8
kf_gt.x = np.array([brown_gt[0,1],brown_gt[0,2],0,0,0,0])
for i in range(1,brown_gt.shape[0]):
    delta_t = brown_gt[i,0] - brown_gt[i-1,0]
    kf_gt.F = np.array([[1,0,delta_t,0,0.5*delta_t**2,0],      # state transition matrix
                        [0,1,0,delta_t,0,0.5*delta_t**2],
                        [0,0,1,0,delta_t,0],
                        [0,0,0,1,0,delta_t],
                        [0,0,0,0,1,0],
                        [0,0,0,0,0,1]])
    kf_gt.predict()
    kf_gt.update(brown_gt[i,1:3])
    brown_gt[i,8:10] = kf_gt.x[2:4]

kf_master = KalmanFilter(dim_x=6, dim_z=2)

kf_master.H = np.array([[1,0,0,0,0,0],
                                [0,1,0,0,0,0]])
# kf.P[2:, 2:] *= 0.5. 	# state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
kf_master.P *= 0.1
kf_master.Q[0:, 0:] *= 0.01
kf_master.R = kf_master.R*0.8
kf_master.x = np.array([master_gt[0,1],master_gt[0,2],master_gt[0,8],master_gt[0,9],0,0])
for i in range(1,master_gt.shape[0]):
    delta_t = master_gt[i,0] - master_gt[i-1,0]
    kf_master.F = np.array([[1,0,delta_t,0,0.5*delta_t**2,0],      # state transition matrix
                     [0,1,0,delta_t,0,0.5*delta_t**2],
                     [0,0,1,0,delta_t,0],
                     [0,0,0,1,0,delta_t],
                     [0,0,0,0,1,0],
                     [0,0,0,0,0,1]])
    kf_master.predict()
    kf_master.update(master_gt[i,1:3])
    master_gt[i,8:10] = kf_master.x[2:4]

#pos error
def pos_error_fun(black_mea, black_gt, master_gt):
    pos_error_radio_black = []
    pos_revalant = np.zeros((black_mea.shape[0],2))
    for i in range(black_mea.shape[0]):
        index = np.where(black_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
        pos_interval_x =  ratio*(black_gt[index_back, 1]  - black_gt[index_front, 1] ) + black_gt[index_front, 1]
        pos_interval_y =  ratio*(black_gt[index_back, 2]  - black_gt[index_front, 2] ) + black_gt[index_front, 2]
        # pos_interval_z =  ratio*(black_gt[index_back, 3]  - black_gt[index_front, 3] ) + black_gt[index_front, 3]

        # error = (pos_interval_x - black_mea[i, 2])**2 + (pos_interval_y - black_mea[i, 3])**2  + (pos_interval_z - black_mea[i, 4] + 0.3)**2  #z pos bias
        error = (pos_interval_x - black_mea[i, 2])**2 + (pos_interval_y - black_mea[i, 3])**2
        error = math.sqrt(error)

        index = np.where(master_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
        master_pos_interval_x =  ratio*(master_gt[index_back, 1]  - master_gt[index_front, 1] ) + master_gt[index_front, 1]
        master_pos_interval_y =  ratio*(master_gt[index_back, 2]  - master_gt[index_front, 2] ) + master_gt[index_front, 2]
        master_pos_interval_z =  ratio*(master_gt[index_back, 3]  - master_gt[index_front, 3] ) + master_gt[index_front, 3]

        # distance_gt = (pos_interval_x - master_pos_interval_x)**2 + (pos_interval_y - master_pos_interval_y)**2  + (pos_interval_z - master_pos_interval_z + 0.1251)**2  #z pos bias
        distance_gt = (pos_interval_x - master_pos_interval_x)**2 + (pos_interval_y - master_pos_interval_y)**2
        # distance_gt = (pos_interval_x)**2 + (pos_interval_y)**2
        distance_gt =math.sqrt(distance_gt)
        pos_error_radio_black.append(float(error/distance_gt))

        #revelan
        pos_revalant[i,0]=(black_mea[i, 2]-master_pos_interval_x)
        pos_revalant[i,1]=(black_mea[i, 3]-master_pos_interval_y)

    return np.array(pos_error_radio_black),pos_revalant

#pos
pos_error_radio_black,_ = pos_error_fun(black_mea, black_gt,master_gt)
print("pos_error_radio_black ", np.mean(pos_error_radio_black))
pos_error_radio_brown,revalant_pos_brown = pos_error_fun(brown_mea, brown_gt,master_gt)
print("pos_error_radio_brown ", np.mean(pos_error_radio_brown))
pos_error_radio_white,_ = pos_error_fun(white_mea, white_gt,master_gt)
print("pos_error_radio_white ", np.mean(pos_error_radio_white))

# vel_revalant = np.zeros((brown_mea.shape[0],3))
# kf = KalmanFilter(dim_x=6, dim_z=2)
# kf.H = np.array([[1,0,0,0,0,0],
#                  [0,1,0,0,0,0]])
# # kf.P[2:, 2:] *= 0.5. 	# state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
# kf.P *= 0.5
# kf.Q[0:, 0:] *= 10
# kf.R = kf.R*1000
# kf.x = np.array([revalant_pos_brown[0,0],revalant_pos_brown[0,1],0,0,0,0])
# for i in range(brown_mea.shape[0]):
#     delta_t = brown_mea[i,0] - brown_mea[i-1,0]
#     kf.F = np.array([[1,0,delta_t,0,0.5*delta_t**2,0],      # state transition matrix
#                      [0,1,0,delta_t,0,0.5*delta_t**2],
#                      [0,0,1,0,delta_t,0],
#                      [0,0,0,1,0,delta_t],
#                      [0,0,0,0,1,0],
#                      [0,0,0,0,0,1]])
#     kf.predict()
#     kf.update(revalant_pos_brown[i,:])
#     vel_revalant[i,1:3] = kf.x[2:4]
#     vel_revalant[i,0] = brown_mea[i,0]

#vel error
# def vel_error_fun(black_mea, black_gt,master_gt):
#     vel_error_radio_black = []
#     error_v = []
#     revalant_gt = np.zeros((black_mea.shape[0]-1,2))
#     for i in range(0, black_mea.shape[0]-1):
#         index = np.where(black_gt[:, 0] > black_mea[i, 0] )
#         index_front = index[0][0] - 1
#         index_back = index[0][0]
#         ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
#         vel_interval_x =  ratio*(black_gt[index_back, 8]  - black_gt[index_front, 8] ) + black_gt[index_front, 8]
#         vel_interval_y =  ratio*(black_gt[index_back, 9]  - black_gt[index_front, 9] ) + black_gt[index_front, 9]
#
#         index = np.where(master_gt[:, 0] > black_mea[i, 0] )
#         index_front = index[0][0] - 1
#         index_back = index[0][0]
#         ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
#         master_vel_interval_x =  ratio*(master_gt[index_back, 8]  - master_gt[index_front, 8] ) + master_gt[index_front, 8]
#         master_vel_interval_y =  ratio*(master_gt[index_back, 9]  - master_gt[index_front, 9] ) + master_gt[index_front, 9]
#         # master_vel_interval_z =  ratio*(master_gt[index_back, 10]  - master_gt[index_front, 10] ) + master_gt[index_front, 10]
#
#         # vel_gt = (vel_interval_x - master_vel_interval_x)**2 + (vel_interval_y - master_vel_interval_y)**2  + (vel_interval_z - master_vel_interval_z)**2
#         vel_gt = (vel_interval_x - master_vel_interval_x)**2 + (vel_interval_y - master_vel_interval_y)**2
#         vel_gt =math.sqrt(vel_gt)
#
#         error = (vel_interval_x - black_mea[i, 1]-master_vel_interval_x)**2 + (vel_interval_y - black_mea[i, 2]-master_vel_interval_y)**2
#         error = math.sqrt(error)
#
#         # black_get_xyz = (vel_interval_x)**2 + (vel_interval_y )**2
#         # black_get_xyz = math.sqrt(black_get_xyz)
#
#         if(vel_gt==0):
#             continue
#         if(vel_gt<0.5):
#             # vel_gt = 0.5
#             error_v.append(error)
#             continue
#         error_rate = float(error/vel_gt)
#         vel_error_radio_black.append(error_rate)
#
#         revalant_gt[i,0] = vel_interval_x - master_vel_interval_x
#         revalant_gt[i,1] = vel_interval_y - master_vel_interval_y
#
#     return np.array(vel_error_radio_black),np.array(error_v),revalant_gt
def vel_error_fun(black_mea, black_gt,master_gt):
    vel_error_radio_black = []
    error_v = []
    for i in range(0, black_mea.shape[0]-1):
        index = np.where(black_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
        vel_interval_x =  ratio*(black_gt[index_back, 8]  - black_gt[index_front, 8] ) + black_gt[index_front, 8]
        vel_interval_y =  ratio*(black_gt[index_back, 9]  - black_gt[index_front, 9] ) + black_gt[index_front, 9]
        # vel_interval_z =  ratio*(black_gt[index_back, 10]  - black_gt[index_front, 10] ) + black_gt[index_front, 10]

        # error = (vel_interval_x - black_mea[i, 5])**2 + (vel_interval_y - black_mea[i, 6])**2  + (vel_interval_z - black_mea[i, 7] )**2
        error = (vel_interval_x - black_mea[i, 5])**2 + (vel_interval_y - black_mea[i, 6])**2
        error = math.sqrt(error)

        index = np.where(master_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
        master_vel_interval_x =  ratio*(master_gt[index_back, 8]  - master_gt[index_front, 8] ) + master_gt[index_front, 8]
        master_vel_interval_y =  ratio*(master_gt[index_back, 9]  - master_gt[index_front, 9] ) + master_gt[index_front, 9]
        # master_vel_interval_z =  ratio*(master_gt[index_back, 10]  - master_gt[index_front, 10] ) + master_gt[index_front, 10]

        # vel_gt = (vel_interval_x - master_vel_interval_x)**2 + (vel_interval_y - master_vel_interval_y)**2  + (vel_interval_z - master_vel_interval_z)**2
        vel_gt = (vel_interval_x - master_vel_interval_x)**2 + (vel_interval_y - master_vel_interval_y)**2
        vel_gt =math.sqrt(vel_gt)

        # black_get_xyz = (vel_interval_x)**2 + (vel_interval_y )**2
        # black_get_xyz = math.sqrt(black_get_xyz)

        if(vel_gt==0):
            continue
        if(vel_gt<0.5):
            vel_gt = 0.5
            continue
        error_rate = float(error/vel_gt)
        # if(error_rate>0.4):
        #     error_rate = 0.4
        vel_error_radio_black.append(error_rate)
        error_v.append(error)

    return np.array(vel_error_radio_black),np.array(error_v)

#vel error
vel_error_radio_black,_ = vel_error_fun(black_mea, black_gt,master_gt)
print("vel_error_radio_black ", np.mean(vel_error_radio_black))
# vel_error_radio_brown,error_v,revalant_gt = vel_error_fun(vel_revalant, brown_gt,master_gt)
vel_error_radio_brown,error_v = vel_error_fun(brown_mea, brown_gt,master_gt)
print("vel_error_radio_brown ", np.mean(vel_error_radio_brown))
vel_error_radio_white,_ = vel_error_fun(white_mea, white_gt,master_gt)
print("vel_error_radio_white ", np.mean(vel_error_radio_white))

# plt.figure(6)
# l1, = plt.plot(np.arange(revalant_gt.shape[0]), revalant_gt[:,0], 'b')
# l2, = plt.plot(np.arange(vel_revalant.shape[0]), vel_revalant[:,1], 'r')
# plt.ylabel('revalant vel')

plt.figure(3)
l1, = plt.plot(np.arange(vel_error_radio_black.shape[0]), vel_error_radio_black, 'b')
plt.ylabel('vel error rate')

plt.figure(4)
l1, = plt.plot(np.arange(error_v.shape[0]), error_v, 'b')
plt.ylabel('vel error')

#pre pos error
def interval_gt(temp_t, temp_t_front, temp_t_back, temp_x_front, temp_x_back, temp_y_front, temp_y_back, temp_z_front, temp_z_back):
    ratio = (temp_t - temp_t_front)/(temp_t_back  - temp_t_front )
    vel_interval_x =  ratio*(temp_x_back  - temp_x_front ) + temp_x_front
    vel_interval_y =  ratio*(temp_y_back  - temp_y_front) + temp_y_front
    vel_interval_z =  ratio*(temp_z_back  - temp_z_front) + temp_z_front
    return vel_interval_x, vel_interval_y, vel_interval_z

def pre_error_fun(black_mea, black_gt,master_gt):
    pos_error_radio_all = []
    for i in range(black_mea.shape[0]):
        time_stamp_base = black_mea[i, 0]

        index = np.where(master_gt[:, 0] > time_stamp_base)
        index_front = index[0][0] - 1
        index_back = index[0][0]
        master_pos_interval_gt_x, master_pos_interval_gt_y, master_pos_interval_gt_z = interval_gt(time_stamp_base, master_gt[index_front, 0], master_gt[index_back, 0],\
                master_gt[index_front, 1], master_gt[index_back, 1],\
                master_gt[index_front, 2], master_gt[index_back, 2],\
                master_gt[index_front, 3], master_gt[index_back, 3])

        index = np.where(black_gt[:, 0] > time_stamp_base)
        index_front = index[0][0] - 1
        index_back = index[0][0]
        pre_pos_interval_gt_x, pre_pos_interval_gt_y, pre_pos_interval_gt_z = interval_gt(time_stamp_base, black_gt[index_front, 0], black_gt[index_back, 0], \
                                                                                          black_gt[index_front, 1], black_gt[index_back, 1], \
                                                                                          black_gt[index_front, 2], black_gt[index_back, 2], \
                                                                                          black_gt[index_front, 3], black_gt[index_back, 3])
        cur_distance_gt = (pre_pos_interval_gt_x-master_pos_interval_gt_x)**2 + (pre_pos_interval_gt_y-master_pos_interval_gt_y)**2   #z pos bias
        cur_distance_gt =math.sqrt(cur_distance_gt)

        pos_error_radio = []
        for j in range(10):
            index = np.where(black_gt[:, 0] > time_stamp_base + (j+1)*0.1 )
            index_front = index[0][0] - 1
            index_back = index[0][0]
            pre_pos_interval_gt_x, pre_pos_interval_gt_y, pre_pos_interval_gt_z = interval_gt(time_stamp_base + (j+1)*0.1, black_gt[index_front, 0], black_gt[index_back, 0], \
                                                                                              black_gt[index_front, 1], black_gt[index_back, 1], \
                                                                                              black_gt[index_front, 2], black_gt[index_back, 2], \
                                                                                              black_gt[index_front, 3], black_gt[index_back, 3])

            error = (pre_pos_interval_gt_x - black_mea[i, 8 + j*3])**2 + (pre_pos_interval_gt_y - black_mea[i, 9 + j*3])**2  #z pos bias
            error = math.sqrt(error)

            pos_error_radio.append(float(error/cur_distance_gt))

        pos_error_radio = np.array(pos_error_radio)
        pos_error_radio_all.append(np.mean(pos_error_radio))

    return np.array(pos_error_radio_all)

#pre pos error
pre_error_radio_black = pre_error_fun(black_mea, black_gt,master_gt)
print("pre_error_radio_black ", np.mean(pre_error_radio_black))
pre_error_radio_brown = pre_error_fun(brown_mea, brown_gt,master_gt)
print("pre_error_radio_brown ", np.mean(pre_error_radio_brown))
pre_error_radio_white = pre_error_fun(white_mea, white_gt,master_gt)
print("pre_error_radio_white ", np.mean(pre_error_radio_white))

data_length_black = min([pos_error_radio_black.shape[0],vel_error_radio_black.shape[0],pre_error_radio_black.shape[0]])
success_black = 0
for i in range(data_length_black):
    if pos_error_radio_black[i] < 0.15 and vel_error_radio_black[i]<0.25:
        success_black += 1
print("success rate ",success_black/data_length_black,"success ",success_black, "data length: ", data_length_black)

data_length_brown = min([pos_error_radio_brown.shape[0],vel_error_radio_brown.shape[0],pre_error_radio_brown.shape[0]])
success_brown = 0
for i in range(data_length_brown):
    if pos_error_radio_brown[i] < 0.15 and vel_error_radio_brown[i]<0.25:
        success_brown += 1
print("success rate ",success_brown/data_length_brown,"success ",success_brown, "data length: ", data_length_brown)

data_length_white = min([pos_error_radio_white.shape[0],vel_error_radio_white.shape[0],pre_error_radio_white.shape[0]])
success_white = 0
for i in range(data_length_white):
    if pos_error_radio_white[i] < 0.15 and vel_error_radio_white[i]<0.25:
        success_white += 1
print("success rate ",success_white/data_length_white,"success ",success_white, "data length: ", data_length_white)

plt.figure(1)
l1, = plt.plot(brown_mea[:,0], brown_mea[:,5], 'b') #v x
l2, = plt.plot(brown_gt[:,0], brown_gt[:,8], 'r-.') #v x
plt.legend(handles=[l1,l2],labels=['Vx_estimated','Vx_gt'])

plt.xlabel('t(s)')
plt.ylabel('Vx(m/s)')


# plt.plot(brown_mea[:,0], brown_mea[:,6], 'b') #v y
# plt.plot(brown_gt[:,0], brown_gt[:,9], 'r-.') #v y
# plt.show()

plt.figure(2)
l1,=plt.plot(brown_mea[:,0], brown_mea[:,2], 'b') #x
l2,=plt.plot(brown_gt[:,0], brown_gt[:,1], 'r-.') #x
plt.legend(handles=[l1,l2],labels=['x_estimated','x_gt'],loc="best")

plt.xlabel('t(s)')
plt.ylabel('x(m)')
plt.show()

# plt.plot(brown_mea[:,0], brown_mea[:,3], 'b') #y
# plt.plot(brown_gt[:,0], brown_gt[:,2], 'r-.') #y
# plt.show()
plt.show()